#include "pca_io.h"
#include <esp_log.h>

#define TAG "PCA_IO"


void PcaIO::init(i2c_master_bus_handle_t i2c_bus, uint8_t addr)
{
    this->m_i2c_addr = addr;
    i2c_device_config_t i2c_config;
    i2c_config.dev_addr_length = I2C_ADDR_BIT_LEN_7;
    i2c_config.device_address = this->m_i2c_addr;
    i2c_config.scl_speed_hz = 100000;
    i2c_config.scl_wait_us = 1000;

    if (this->m_i2c_dev == NULL) {
        i2c_master_bus_add_device(i2c_bus, &i2c_config, &this->m_i2c_dev);
    }

    if (!this->m_i2c_dev) {
        ESP_LOGE(TAG, "Failed to add I2C device");
        return;
    }
}


void PcaIO::deinit()
{
    i2c_master_bus_rm_device(this->m_i2c_dev);
    this->m_i2c_dev = NULL;
}


int PcaIO::write_pin(int pin, PinValue value)
{
    uint8_t reg_value = 0;
    if (read_reg(PCA9557_CONTROL_REG_1, &reg_value, 1) != 0) {
        ESP_LOGE(TAG, "Failed to read register");
        return -1;
    }

    if (value == PIN_HIGH) {
        reg_value |= (1 << pin);
    } else {
        reg_value &= ~(1 << pin);
    }

    if (write_reg(PCA9557_CONTROL_REG_1, reg_value) != 0) {
        ESP_LOGE(TAG, "Failed to write register");
        return -1;
    }
    return 0;
}

PcaIO::PinValue PcaIO::read_pin(int pin)
{
    uint8_t reg_value = 0;
    if (read_reg(PCA9557_CONTROL_REG_0, &reg_value, 1) != 0) {
        return PIN_UNKNOWN;
    }

    return (reg_value & (1 << pin)) ? PIN_HIGH : PIN_LOW;
}

int PcaIO::set_pin_dir(int pin, PinDir dir)
{
    uint8_t reg_value = 0;
    if (read_reg(PCA9557_CONTROL_REG_3, &reg_value, 1) != 0) {
        return -1;
    }

    if (dir == PIN_OUTPUT) {
        reg_value &= ~(1 << pin);
    } else {
        reg_value |= (1 << pin);
    }

    if (write_reg(PCA9557_CONTROL_REG_3, reg_value) != 0) {
        return -1;
    }
    return 0;
}

int PcaIO::set_intput_polarity(int pin, PinPolarity polarity)
{
    uint8_t reg_value = 0;
    if (read_reg(PCA9557_CONTROL_REG_2, &reg_value, 1) != 0) {
        return -1;
    }

    if (polarity == PIN_INVERTED) {
        reg_value |= (1 << pin);
    } else {
        reg_value &= ~(1 << pin);
    }

    if (write_reg(PCA9557_CONTROL_REG_2, reg_value) != 0) {
        return -1;
    }
    return 0;
}

int PcaIO::write_reg(uint8_t cmd, uint8_t value)
{
    uint8_t data[2];
    data[0] = cmd;
    data[1] = value;
    if(ESP_OK != i2c_master_transmit(this->m_i2c_dev, data, 2, 1000)) {
        ESP_LOGE(TAG, "Failed to write register");
        return -1;
    }
    return 0;
}

int PcaIO::read_reg(uint8_t cmd, uint8_t *value, uint8_t count)
{
    if (ESP_OK != i2c_master_transmit_receive(this->m_i2c_dev, &cmd, 1, value, count, 1000)) {
        ESP_LOGE(TAG, "Failed to read register");
        return -1;
    }
    return 0;
}
